#include "uart_control.h"

/**********************************************************
***	Emm_V5.0步进闭环控制例程
***	编写作者：fan-an9
**********************************************************/

#define accel 0x03  // 加速度

/**
 * @brief 发送位置控制命令（绝对位置）
 * @param id 驱动器地址
 * @param pulses 脉冲数（小端序，如3200=0x00000C80）
 * @param dir 方向：0=CW, 1=CCW
 * @param speed_rpm 速度（如2000=0x07D0）
 * @param sync_flag 多机同步：0=禁用, 1=启用
 */
void set_position(uint8_t id, int32_t pulses, uint8_t dir, uint16_t speed_rpm, uint8_t sync_flag) {
    uint8_t cmd[13] = {
        id, 0xFD, dir, 
        (uint8_t)(speed_rpm >> 8), (uint8_t)(speed_rpm >> 0),   // 速度
        accel,                                       // 加速度
        (uint8_t)(pulses >> 24), (uint8_t)(pulses >> 16), (uint8_t)(pulses >> 8), (uint8_t)(pulses >> 0),        // 脉冲数
        0x01,                                       // 绝对位置
        sync_flag,                                  // 同步标志
        0x6B                                        // 校验（必须最后一位）
    };
    HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
}

/** b 
 * @brief 控制电机使能状态
 * @param id 驱动器地址（1-255，0为广播地址）
 * @param enable 使能标志：
 *               - true(1)：使能电机
 *               - false(0)：失能电机
 * @param sync_flag 多机同步标志：
 *                 - 0x00：单机控制
 *                 - 0x01：多机同步（需配合同步命令0xFF 0x66）
 * @note 命令格式：
 *       [地址][功能码F3 AB][使能状态][同步标志][校验码6B]
 * @warning 需确保驱动器已配置为串口控制模式（P_Serial=UART_FUN）
 * @example 
 *   motor_enable(1, true, 0);   // 单独使能地址1的电机
 *   motor_enable(0, true, 1);   // 广播使能所有电机（需后续发同步命令）
 */
void motor_enable(uint8_t id, bool enable, uint8_t sync_flag) 
{
    uint8_t cmd[6] = {
        id,        // 地址
        0xF3,      // 功能码高位
        0xAB,      // 功能码低位
        enable ? 0x01 : 0x00,  // 使能状态
        sync_flag, // 同步标志
        0x6B       // 固定校验码
    };
    
    HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
}

/**
 * @brief 发送多机同步运动触发命令
 * @param id 目标地址：
 *           - 0x00: 广播地址（所有电机同步）
 *           - 1-255: 指定电机单独同步
 * @note 命令格式：
 *       [地址][功能码FF][同步码66][校验码6B]
 * @warning 需先发送带同步标志的位置/速度命令（sync_flag=1）
 * @example 
 *   // 多机同步运动流程：
 *   set_position(1, 3200, 0, 1);  // 电机1准备运动（sync_flag=1）
 *   set_position(2, 6400, 1, 1);  // 电机2准备运动（sync_flag=1）
 *   sync_motion(0);               // 广播触发同步运动
 */
void sync_motion(uint8_t id) 
{
    uint8_t cmd[4] = {
        id,    // 地址
        0xFF,  // 功能码
        0x66,  // 同步触发码
        0x6B   // 固定校验码
    };
    
    HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
}

/**
 * @brief 清零电机当前位置角度和脉冲计数
 * @param id 驱动器地址（1-255，0为广播地址）
 * @note 命令格式：
 *       [地址][功能码0A 6D][校验码6B]
 * @warning 此操作会清除驱动器的内部位置记录
 * @example 
 *   clear_position(1);  // 清零地址1的电机位置
 */
void clear_position(uint8_t id)
{
    uint8_t cmd[4] = {
        id,        // 地址
        0x0A,      // 功能码高位
        0x6D,      // 功能码低位
        0x6B       // 固定校验码
    };
    
    HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
}

/**
 * @brief 获取当前的电机位置
 * @param id 驱动器地址（1-255，0为广播地址）
 * @example get_position(1);  // 获取电机1的当前位置
 */
void get_position(uint8_t id)
{
    uint8_t cmd[3] = {
        id,        // 地址
        0x36,      // 功能码高位
        0x6B       // 固定校验码
    };
    
    HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
    
    // 解算示例
    // if (rx_buf[0] == 0x01 && rx_buf[1] == 0x36) { // 位置查询响应
    //   uint8_t direction = (rx_buf[2] == 0x00) ? '+' : '-';
    //   printf("Current position: %c%d\r\n", direction, 
    //          ((rx_buf[3] << 24) | (rx_buf[4] << 16) | (rx_buf[5] << 8) | rx_buf[6]));
    // }
}

/**
 * @brief 生成XY平台运动指令（指哪打哪）
 * @param x X轴目标位置（脉冲数，正数=CW，负数=CCW）
 * @param y Y轴目标位置（脉冲数，正数=CW，负数=CCW）
 * @param speed_rpm 运动速度（RPM）
 * @note X轴电机ID=1，Y轴电机ID=2
 * @note 如果有加速度的存在，发送指令频率越快，电机工作越慢(减少发送频率)
 * @example 
 *   generate_motor_commands(1000, -2000, 50);  // X轴正转1000脉冲，Y轴反转2000脉冲，速度50RPM
 */
void generate_motor_commands(int32_t x, int32_t y, uint16_t speed_rpm) {
    // X轴运动（ID=1），方向由x正负决定
    set_position(1, abs(x), (x < 0) ? 1 : 0, speed_rpm, 0);
    HAL_Delay(10);
    
    // Y轴运动（ID=2），方向由y正负决定
    set_position(2, abs(y), (y < 0) ? 1 : 0, speed_rpm, 0);
    HAL_Delay(10);
}

/**
 * @brief 电机停止运动
 * @param addr 电机地址
 * @param sync_flag 多机同步标志位
 */
void motor_stop(uint8_t addr, uint8_t sync_flag)
{
  uint8_t cmd[16] = {0};
  
  // 装载命令
  cmd[0] =  addr;                       // 地址
  cmd[1] =  0xFE;                       // 功能码
  cmd[2] =  0x98;                       // 辅助码
  cmd[3] =  sync_flag;                  // 多机同步运动标志
  cmd[4] =  0x6B;                       // 校验字节
  
  // 发送命令
  HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
}

/**
 * @brief 强制中断并退出回零
 * @param addr 电机地址
 * @note X轴电机ID=1，Y轴电机ID=2
 */
void motor_interrupt_zero(uint8_t addr)
{
  uint8_t cmd[16] = {0};
  
  // 装载命令
  cmd[0] =  addr;                       // 地址
  cmd[1] =  0x9C;                       // 功能码
  cmd[2] =  0x48;                       // 辅助码
  cmd[3] =  0x6B;                       // 校验字节
  
  // 发送命令
  HAL_UART_Transmit(&huart2, cmd, sizeof(cmd), 100);
}

#if 0
/**
 * @brief 电机画圆
 * @param speed_rpm 电机速度（单位：RPM）推荐100
 * @example motor_circle(50);  // 控制电机循环运动，速度50
 */
void motor_circle(uint16_t speed_rpm){
    for (int i = 0; i < 2000; i = i+50)
    {
      // generate_motor_commands(i, i, 1500);
      set_position(1, i, 0, speed_rpm, 0);
      HAL_Delay(10);
      set_position(2, i, 0, speed_rpm, 0);
      HAL_Delay(10);
    }
    for (int i = 2000; i > 0; i = i-50)
    {
      // generate_motor_commands(i, i, 1500);
      set_position(1, i, 0, speed_rpm, 0);
      HAL_Delay(10);
      set_position(2, i, 1, speed_rpm, 0);
      HAL_Delay(10);
    }
    for (int i = 0; i < 2000; i = i+50)
    {
      // generate_motor_commands(i, i, 1500);
      set_position(1, i, 1, speed_rpm, 0);
      HAL_Delay(10);
      set_position(2, i, 1, speed_rpm, 0);
        HAL_Delay(10);
    }
    for (int i = 2000; i > 0; i = i-50)
    {
      // generate_motor_commands(i, i, 1500);
      set_position(1, i, 1, speed_rpm, 0);
      HAL_Delay(10);
      set_position(2, i, 0, speed_rpm, 0);
        HAL_Delay(10);
    }
}

/* 私有函数：计算运动预估时间（ms）*/
static uint32_t calculate_delay(float x, float y, uint16_t speed) 
{
    float dist = sqrtf(x*x + y*y);  // 运动距离（mm）
    return (uint32_t)(dist * 20 / (speed + 1));  // 经验公式，需实际校准
}

/**
 * @brief 生成XY平台运动指令（指哪打哪）
 * @param x X轴目标坐标（单位：mm）
 * @param y Y轴目标坐标（单位：mm）
 * @param speed 运动速度（默认256，范围0-1000，数值越大越快）
 * @note 依赖函数：
 *       - set_position() 用于发送脉冲命令
 *       - motor_enable() 用于电机使能
 * @warning 需提前配置：
 *          - 脉冲当量（如1mm=100脉冲）
 *          - 电机运动方向校准
 */
void generate_motor_commands(float x, float y, uint16_t speed) 
{
    // ============= 参数配置 =============
    const float PULSE_PER_MM = 100.0f;  // 每毫米对应的脉冲数（根据实际机械结构调整）
    const uint8_t MOTOR_ID_X = 1;        // X轴电机地址
    const uint8_t MOTOR_ID_Y = 2;        // Y轴电机地址
    
    // ============= 运动控制 =============
    // 1. 使能电机
    motor_enable(MOTOR_ID_X, true, 0);
    motor_enable(MOTOR_ID_Y, true, 0);
    
    // 2. 计算脉冲数（带方向判断）
    int32_t pulses_x = (int32_t)(x * PULSE_PER_MM);
    int32_t pulses_y = (int32_t)(y * PULSE_PER_MM);
    
    // 3. 速度映射（将0-1000转换为驱动器速度值）
    uint16_t motor_speed = 256 + (speed * 5);  // 映射到256-5256范围
    
    // 4. 发送运动指令（简化版，实际需拆分速度到命令）
    printf("Moving to (%.1f, %.1f) mm\n", x, y);
    set_position(MOTOR_ID_X, abs(pulses_x), (pulses_x > 0) ? 0 : 1);
    set_position(MOTOR_ID_Y, abs(pulses_y), (pulses_y > 0) ? 0 : 1);
    
    // 5. 等待到位（实际应根据驱动器反馈或定时）
    HAL_Delay(calculate_delay(x, y, speed));
}
#endif
